#include "ceres/ceres.h"
#include "glog/logging.h"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;

// 这个是使用tools目录的python脚本生成的 f(x) = 2x + 1 函数带高斯噪声的点
double x_data[] = {0., 0.1010101, 0.2020202, 0.3030303, 0.4040404,
                   0.50505051, 0.60606061, 0.70707071, 0.80808081, 0.90909091,
                   1.01010101, 1.11111111, 1.21212121, 1.31313131, 1.41414141,
                   1.51515152, 1.61616162, 1.71717172, 1.81818182, 1.91919192,
                   2.02020202, 2.12121212, 2.22222222, 2.32323232, 2.42424242,
                   2.52525253, 2.62626263, 2.72727273, 2.82828283, 2.92929293,
                   3.03030303, 3.13131313, 3.23232323, 3.33333333, 3.43434343,
                   3.53535354, 3.63636364, 3.73737374, 3.83838384, 3.93939394,
                   4.04040404, 4.14141414, 4.24242424, 4.34343434, 4.44444444,
                   4.54545455, 4.64646465, 4.74747475, 4.84848485, 4.94949495,
                   5.05050505, 5.15151515, 5.25252525, 5.35353535, 5.45454545,
                   5.55555556, 5.65656566, 5.75757576, 5.85858586, 5.95959596,
                   6.06060606, 6.16161616, 6.26262626, 6.36363636, 6.46464646,
                   6.56565657, 6.66666667, 6.76767677, 6.86868687, 6.96969697,
                   7.07070707, 7.17171717, 7.27272727, 7.37373737, 7.47474747,
                   7.57575758, 7.67676768, 7.77777778, 7.87878788, 7.97979798,
                   8.08080808, 8.18181818, 8.28282828, 8.38383838, 8.48484848,
                   8.58585859, 8.68686869, 8.78787879, 8.88888889, 8.98989899,
                   9.09090909, 9.19191919, 9.29292929, 9.39393939, 9.49494949,
                   9.5959596, 9.6969697, 9.7979798, 9.8989899, 10.};
double y_data[] = {0.93437037, 1.10800778, 1.49749759, 1.62483486, 1.85745427,
                   1.79459743, 2.27304958, 2.43684514, 2.59498472, 2.90145584,
                   3.02235446, 3.29461151, 3.40512373, 3.58855956, 3.77024815,
                   3.91124955, 4.22780088, 4.53374423, 4.85377801, 4.84796088,
                   5.14480467, 5.12243678, 5.35482069, 5.53019196, 5.87406981,
                   6.13973843, 6.20375315, 6.24188504, 6.68239248, 6.93160315,
                   6.99614076, 7.15055413, 7.4311195, 7.54384731, 7.89796158,
                   7.97516088, 8.22368618, 8.43758254, 8.70023989, 8.8775146,
                   9.02566325, 9.33349747, 9.49662813, 9.67447582, 9.90088601,
                   10.17162523, 10.33484054, 10.38349433, 10.7364018, 10.79706869,
                   11.17183227, 11.37198439, 11.45395771, 11.80361811, 11.93798479,
                   12.06102539, 12.34515041, 12.697212, 12.78506384, 12.93441891,
                   13.2612334, 13.33135895, 13.73069388, 13.83714134, 14.01940107,
                   14.13475724, 14.35964588, 14.60808308, 14.75824609, 14.89877116,
                   15.00697691, 15.24390352, 15.44755703, 15.77805894, 15.93415946,
                   16.09605892, 16.33307107, 16.53059336, 16.74610758, 16.92441717,
                   17.03073718, 17.3953127, 17.5472001, 17.91327728, 17.91618655,
                   18.22026001, 18.39197993, 18.5505761, 18.91135432, 18.92275376,
                   19.13110282, 19.27695372, 19.48347767, 19.87969754, 19.87926354,
                   20.27262121, 20.37837706, 20.81583789, 20.79911681, 20.96614921};

class LineResidualFunction
{
public:
  LineResidualFunction(double x, double y) : x_(x), y_(y) {}

  template <typename T>
  bool operator()(const T *const k, const T *const b, T *r) const // 容易错，const位置很关键
  {
    r[0] = y_ - (k[0] * x_ + b[0]);
    return true;
  }

private:
  const double x_;
  const double y_;
};

int main(int argc, char **argv)
{
  int kNumObservations = sizeof(x_data) / sizeof(double);

  // f(x) = k * x + b
  double k = 0;
  double b = 0;

  Problem problem;
  for (int i = 0; i < kNumObservations; ++i)
  {
    problem.AddResidualBlock(
        new AutoDiffCostFunction<LineResidualFunction, 1, 1, 1>(
            new LineResidualFunction(x_data[i], y_data[i])), // 代价函数cost function
        nullptr,                                             // 损失函数，这里不懂。写默认值试试效果
        // 接下来是数据块。
        &k,
        &b);
  }

  Solver::Options options;
  options.max_num_iterations = 25;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;

  Solver::Summary summary;
  Solve(options, &problem, &summary);
  std::cout << summary.BriefReport() << "\n";
  std::cout << "Use time(second): " << summary.total_time_in_seconds << "\n";
  std::cout << "Final   m: " << k << " c: " << b << "\n";
  return 0;
}
